#ifndef _DRIVEBASE_ROBOT_H
#define _DRIVEBASE_ROBOT_H

#include "WPILib.h"

#include "EntechSubsystem.h"
#include "Entech2013DriveSubsystem.h"
#include "DumperSubsystem.h"
#include "ClimberSubsystem.h"
#include "TilterSubsystem.h"
#include "OperatorButton.h"

#define ENTECH_ROBOT   dynamic_cast <EnTech2013Robot &>(RobotBase::getInstance())

class EnTech2013Robot : public IterativeRobot
{	
public:
/**
 * Constructor for this "BuiltinDefaultCode" Class.
 * 
 * The constructor creates all of the objects used for the different inputs and outputs of
 * the robot.  Essentially, the constructor defines the input/output mapping for the robot,
 * providing named objects for each of the robot interfaces. 
 */
	EnTech2013Robot(void);	
	
	/********************************** Init Routines *************************************/
	void RobotInit(void);
	void DisabledInit(void);
	void AutonomousInit(void);
	void TeleopInit(void);
	void TestInit(void);
	
	/********************************** Periodic Routines *************************************/
	void DisabledPeriodic(void);
	void AutonomousPeriodic(void);
	void TeleopPeriodic(void);
	void TestPeriodic(void);
	
	// ********* Entech Specific Methods *******************************
	void RegisterSubsystem(EntechSubsystem *subsys);
	Entech2013DriveSubsystem *GetDrive(void) { return m_robotDrive; };
	DumperSubsystem *GetDumper(void) { return m_dumper; };
	TilterSubsystem *GetTilter(void) { return m_tilter; };
	
private:
	// Store pointers to various robot subsystems
	const static int c_maxSubsystems = 512;
	EntechSubsystem *m_subsystems[c_maxSubsystems];
	int m_subsystemCount;

	// Declare variable for the robot drive system
	Entech2013DriveSubsystem *m_robotDrive;		// robot will use PWM 1-4 for drive motors
	DumperSubsystem  *m_dumper;
	TilterSubsystem  *m_tilter;
	ClimberSubsystem *m_climber;
	Compressor       *m_compressor;
	
	// Variables for running subsystem tests
	int m_testSubsys;
	int m_testNumber;
	
	// Declare a variable to use to access the driver station object
	DriverStation *m_ds;						// driver station object
	DriverStationLCD *m_dsLCD;					// driver station LCD display
	
    // Input Sources
    Joystick *m_leftStick;
    Joystick *m_rightStick;
    OperatorButton *m_shiftHighButton;
    OperatorButton *m_shiftLowButton;
    OperatorButton *m_toggleDriveTypeButton;
    OperatorButton *m_forceDriveOnButton;

    //  Dumper buttons
	OperatorButton *m_kickButton;
	OperatorButton *m_armButton;
	OperatorButton *m_startButton;
	
	// Climber buttons
	OperatorButton *m_climberDeadman;
	OperatorButton *m_climberLeftForward;
	OperatorButton *m_climberLeftBackward; 
	OperatorButton *m_climberRightForward;
	OperatorButton *m_climberRightBackward;

	//  Tilter button
	OperatorButton *m_tilterButton;
	
	// Autonomous Variables
	enum AutoState { kWait0, kWait1, kWait2, 
		kP0Drive1, kP0Drive2, kP0Drive3, kP0Drive4, kP0Drive5, kP0Drive6,
		kP1Drive1, kP1Drive2, kP1Drive3, kP1Drive4, kP1Drive5,
		kP2Drive1, kP2Drive2, kP2Drive3, kP2Drive4, kP2Drive5, kP2Drive6, kP2Drive7,
		kKick, kKickDown, 
		kE0Drive1, kE0Drive2, kE0Drive3, 
		kE1Drive1, kE1Drive2, kE1Drive3, 
		kDone };
	AutoState m_autoState;
	bool m_newState;
	Timer *m_autoTimer;
	int m_startPosition;
	int m_exitPath;
	// Maximimum wait times for each starting position
	float m_waitDelay;
	const static float c_wait0 = 5.0;
	const static float c_wait1 = 4.0;
	const static float c_wait2 = 3.0;
};

#endif
